#include<ros/ros.h>
#include<visualization_msgs/Marker.h>
#include<iostream>
#include<cmath>
#include<thread>
#include<eigen3/Eigen/Dense>
#include<opencv2/opencv.hpp>
#include<opencv2/core/eigen.hpp>

using namespace std;
using namespace cv;
using namespace ros;
using namespace Eigen;
Mat frame,bin,src;
Mat raux,taux;
Mat Rvec,Tvec;
double thx,thy,thz;
Quaterniond q;
Matrix<float, Dynamic, Dynamic> T(3,1);
void frames()
{
    //frame=imread("1.jpg");
	bin = frame;
	vector<Mat> channels;
	split(frame, channels);
	src = channels.at(2) - channels.at(0);
	threshold(src, src, 80, 255, THRESH_BINARY);
	vector<vector<Point>> contours;
    Point2f v[4];Point2f v1[4];Point2f vv[4];
	RotatedRect rrectA;RotatedRect rrect;
	vector<RotatedRect> rrects;
	findContours(src, contours, RETR_LIST, CHAIN_APPROX_NONE);
	for (size_t i = 0; i < contours.size(); i++)
	{
		vector<Point> points;
		double area = contourArea(contours[i]);
		if (area<100 || area>1e4) continue;
		drawContours(src, contours, (int)i, Scalar(0), 2);
		points = contours[i];
		
		if(points.size()>=5) rrect = fitEllipse(points);
		else rrect = minAreaRect(points);
		
		rrect.points(v);
		rrects.push_back(rrect);
	}
	sort(rrects.begin(),rrects.end(),[](const RotatedRect &a,const RotatedRect &b)
	{
		return a.center.x<b.center.x;
	});
	rrects[0].points(v);rrects[1].points(v1);
	vv[0].x=(v[0].x+v[3].x)*0.5;vv[0].y=(v[0].y+v[3].y)*0.5;
	vv[1].x=(v[1].x+v[2].x)*0.5;vv[1].y=(v[1].y+v[2].y)*0.5;
	vv[2].x=(v1[1].x+v1[2].x)*0.5;vv[2].y=(v1[1].y+v1[2].y)*0.5;
	vv[3].x=(v1[0].x+v1[3].x)*0.5;vv[3].y=(v1[0].y+v1[3].y)*0.5;
	Point tmp;
	if(vv[0].y>vv[1].y) {tmp=vv[1];vv[1]=vv[0];vv[0]=tmp;}
	if(vv[2].y<vv[3].y) {tmp=vv[2];vv[2]=vv[3];vv[3]=tmp;}
	for(int i=0;i<4;i++)
	{
		line(bin,vv[i],vv[(i+1)%4],Scalar(255,0,0),4);
	}
	vector<Point3f> inpoint;
	vector<Point2f> imgp;
	inpoint.push_back(Point3f(-675.0,275.0,0.0));
	inpoint.push_back(Point3f(675.0,275.0,0.0));
	inpoint.push_back(Point3f(675.0,-275.0,0.0));
	inpoint.push_back(Point3f(-675.0,-275.0,0.0));
	Mat inpointM;
	Mat(inpoint).convertTo(inpointM,CV_32F);
	for(int i=1;i<=4;i++)
		imgp.push_back(vv[i%4]);
	// cout<<imgp<<endl;
	double camD[9] = {1128.048344, 0, 339.421769,
					0, 1127.052190, 236.535242,
					0, 0, 1};
	double distCoeffD[5] = {-0.568429, 0.514592, -0.000126, 0.000500, 0.000000};
	Mat cm = Mat(3,3,CV_64FC1,camD);
	Mat dc = Mat(5,1,CV_64FC1,distCoeffD);
	solvePnP(inpointM,Mat(imgp),cm,dc,raux,taux,false,SOLVEPNP_P3P);
	raux.convertTo(Rvec,CV_32F);
	taux.convertTo(Tvec,CV_32F);
	Mat_<float> rotMat(3, 3);
    Rodrigues(Rvec, rotMat);
 	Matrix3d R(3,3);
    
	cv2eigen(rotMat, R);
    cv2eigen(Tvec, T);
	q=R;
	// thz=atan2(R(2-1,1-1),R(1-1,1-1));
	// thy=atan2(-R(3-1,1-1),sqrt(R(3-1,2-1)*R(3-1,2-1)+R(3-1,3-1)*R(3-1,3-1)));
	// thx=atan2(R(3-1,2-1),R(3-1,3-1));
	// cout<<T(1,0)<<endl;
	// cout<<thx<<endl;
	imshow("bin", bin);
	//imshow("src", src);
	waitKey(30);
}
void video()
{
    VideoCapture cap;
	cap.open("/home/lfz_5/文档/robot/Files/water.avi");
	for (;;)
	{
		cap.read(frame);
		if (frame.empty()) return;
		frames();
	}
}
int main(int argc, char** argv )
{
  init(argc, argv, "vis");
  NodeHandle n;
  Rate r(1);
  Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
//   thread Thread(video);Thread.detach();
  
  // Set our initial shape type to be a cube
  uint32_t shape = visualization_msgs::Marker::CUBE;
  VideoCapture cap;
  cap.open("/home/lfz_5/文档/robot/Files/water.avi");
  while (ok())
  {
	cap.read(frame);
	if (frame.empty()) return 0; 
	frames();
	visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "map";
    // marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "basic_shapes";
    marker.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;
	double pi=3.1415926;
    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    marker.pose.position.x = -T(0,0)/1e3;
    marker.pose.position.y = -T(2,0)/1e3;
    marker.pose.position.z = T(1,0)/1e3;
	// marker.pose.position.x = Tvec.at<double>(0)*1e-3;
	// marker.pose.position.y = Tvec.at<double>(1)*1e-3;
	// marker.pose.position.z = Tvec.at<double>(2)*1e-3;
    marker.pose.orientation.x = q.x();
    marker.pose.orientation.y = q.y();
    marker.pose.orientation.z = q.z();
    marker.pose.orientation.w = q.w();

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 1.35;
    marker.scale.y = 0.55;
    marker.scale.z = 0.01;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration();

    // Publish the marker
    while (marker_pub.getNumSubscribers() < 1)
    {
      if (!ros::ok())
      {
        return 0;
      }
      ROS_WARN_ONCE("Please create a subscriber to the marker");
      
    }
    marker_pub.publish(marker);
  }
}
  
